#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy, sys
from time import sleep
from sg805_driver.msg import FastPoint



class FastPointDemo:
    def __init__(self):
        rospy.init_node('fastPointCommander', anonymous = False )
        pub = rospy.Publisher("/sg805/fastPoint", FastPoint, queue_size=10)
        fastPointMsg = FastPoint()
        sleep(2)    # 建立连接之后不能马上发送数据



        #根据功能编号进行调用，参考 sg805RobotRos中 fastPointCB回调函数的内容。
        fastPointMsg.Mode = 0       # 0: 绝对移动,机械臂各个轴 移动到某一角度
        #                             # 1: 增量移动,机械臂各个轴 移动一定角度
        fastPointMsg.duration = 4000
        fastPointMsg.Position.append(-1024)
        fastPointMsg.Position.append(-1024)
        fastPointMsg.Position.append(-1024)
        fastPointMsg.Position.append(-1024)
        fastPointMsg.Position.append(-1024)
        fastPointMsg.Position.append(-1024) 
        pub.publish(fastPointMsg)
        sleep(5)

        # 第5轴旋转90度 -1024-50*2048/4 = -26624
        fastPointMsg.Position[4] = -26624
        pub.publish(fastPointMsg)





if __name__ == '__main__':
    try:
        handle = FastPointDemo()
    except rospy.ROSInterruptException:
        pass